/*
 * Particle.h
 *
 *  Created on: 23.11.2011
 *      Author: jan
 */

#ifndef PARTICLE_H_
#define PARTICLE_H_
#include <cmath>
#include <cstdlib>
#include <vector>
#include "AbstractMap.h"
#include "Point.h"
#include "MathUtil.h"

namespace std {

template <class T>
class Particle {
public:
	double dx;
	double dy;
	double x;
	double y;
	Point p;
	double heading;
	double w;
	static double sigma2;
	static double little_noise;
	static double some_noise;

	Particle():dx(0),dy(0),x(0),y(0),p(),heading(0.0),w(0){}
	Particle(Point p):dx(0),dy(0),x(p.x),y(p.y),p(p),heading(0.0),w(0){}

	Particle(const Particle& pa){
		dx = pa.dx;
		dy = pa.dy;
		x = pa.x;
		y = pa.y;
		p = pa.p;
		heading = pa.heading;
		w = pa.w;
	}

	Particle& operator=(const Particle &pa){
		dx = pa.dx;
		dy = pa.dy;
		x = pa.x;
		y = pa.y;
		p = pa.p;
		heading = pa.heading;
		w = pa.w;
		return *this;
	}

	static Point compute_mean_point(vector<Particle > *parts, int particle_nbr, AbstractMap<T> *maze){
		bool noisy = false;
		double m_x = 0.0;
		double m_y = 0.0;
		double m_count = 0.0;
		for(unsigned int i = 0; i < parts->size(); i++){
			m_count += (*parts)[i].w;
			m_x += (*parts)[i].x * (*parts)[i].x;
			m_y += (*parts)[i].y * (*parts)[i].y;
		}
		if(m_count == 0) return Point(-1,-1);
		m_x /= m_count;
		m_y /= m_count;
		m_count = 0;
		Point m(m_x,m_y);
		for (unsigned int i = 0; i < parts->size(); i++) {
			if (AbstractMap<T>::distance((*parts)[i].p,m) < 1.0){
				m_count ++;
			}
		}
		noisy = m_count > (particle_nbr * 0.95);
		return Point(m_x,m_y);
	}

	static double add_noise(double border, double val){
		return val+(((UNIFORM_RAND)*2*border)-border);
	}

	static double add_little_noise(double val){
		return add_little_noise(val,1);
	}

	static double add_little_noise(double val,double resol){
		return add_noise(little_noise*resol,val);
	}
	static double add_some_noise(double val){
		return add_noise(some_noise,val);
	}

	static double w_gauss(double a, double b){
		double error = a - b;
		return pow(M_PI,-(error*error/(2*sigma2)));
	}

	static double w_gauss(double a, double b, double resol){
		double error = a - b;
		return pow(M_PI,-(error*error/(2*(sigma2*resol))));
	}

	Particle(double x, double y, double heading,double w): x(x),y(y),p(x,y),heading(heading),w(w) {

	}

	virtual ~Particle() {

	}
	static void create_dummy(vector<Particle<T> >* p, int count, AbstractMap<T> * map){
		for (int i = 0; i < count; i++) {
			p->push_back(Particle(Point(-1,-1)));
		}
	}

	static void create_random(vector<Particle<T> >* p, int count, AbstractMap<T> * map){
		int i = 0;
		for( i = 0; i < count; i++){
			Point poin = map->random_free_place();
//			Particle o(poin);
			p->push_back(Particle(poin));
		}
	}

	void move_by(double x, double y){
		this->x += x;
		this->y += y;
		this->p = Point(this->x,this->y);
	}

	double read_sensor(AbstractMap<T> * map){
		return map->distance_to_nearest_beacon(p);
	}

	bool advance_by(double speed,AbstractMap<T> * m){
		return advance_by(speed,m,1,true,true);
	}
	bool advance_by(double speed,AbstractMap<T> * m, double resol){
			return advance_by(speed,m,resol,true,true);
	}
	bool advance_by(double speed, AbstractMap<T> * m, bool noisy){
		return advance_by(speed,m,1,noisy,false);
	}
	bool advance_by(double speed, AbstractMap<T> * m,double resol, bool noisy,bool no_checker){
		double h = this->heading;
		if(noisy){
			speed = add_little_noise(speed,resol);
			h = add_little_noise(h,resol);
			h += (((UNIFORM_RAND)*6)-(3));
		}
		double r = radians(h);
		this->dx = cos(r) * speed;
		this->dy = sin(r) * speed;
		Point poi (x+dx,y+dy);
		if(!no_checker){
			no_checker = m->is_free(poi);
		}
		if(no_checker){
			this->move_by(dx,dy);
			return true;
		}
		return false;
	}
};

template<class T>
double Particle<T>::sigma2 = 0.9*0.9;
template<class T>
double Particle<T>::little_noise = 0.02;
template<class T>
double Particle<T>::some_noise = 0.1;

} /* namespace std */
#endif /* PARTICLE_H_ */
